#include "STMFCT.h"
#include "OLED.h"
#include "PWM.h"
#include "encoder.h"
#include "task.h"
#include "k210.h"
#include "flash.h"
#include "MPU6050.h"
#include "GPIO.h"
#include "OPENMV.h"
#include "NRF24L01.h"
#include "GraySensor.h"
#include "POWER.h"
#include "UART.h"

// array2[2] = {3, 8};;

int task_state = 1;
uint8_t TASK_Flag = 0;
uint8_t Yao_Flag = 2;
uint8_t old_Yao_Flag = 2;
uint16_t i = 0;
uint8_t numberfrom_k210 = 0;
uint8_t read_flash = 1;

#define TIM 1
#define MPU6050_T 2
#define L298N 3
#define OPENMV_T 4
#define NORMAL 5
#define NRF24L01_T 6
#define K210_T 7
#define GraySensor_T 8
#define POWER 9

#define TEST NRF24L01_T

#if TEST == GraySensor_T

int main()
{
    uint8_t i = 0;
    uint16_t dat1;
    uint16_t dat2;
    bool dat3 = false;
    GraySensor_init();
    OLED_Init();
    while (1)
    {
        i++;
        dat1 = GraySensor_getA(ADC1);
        dat2 = GraySensor_getA(ADC2);
        dat3 = GraySensor_getD();
        OLED_ShowNum(4, 1, i, 3);
        OLED_ShowNum(1, 1, dat1, 5);
        OLED_ShowNum(2, 1, dat2, 5);
        OLED_ShowNum(3, 1, dat3, 5);
    }
}

#endif

#if TEST == K210_T

int main()
{
    uint8_t *str1 = NULL;
    uint8_t i = 0;
    K210_Init(USART3, 115200);
    OLED_Init();
    while (1)
    {
        i++;
        OLED_ShowNum(4, 1, i, 3);
        if (K210_receiveString(&str1))
        {
            K210_unZip(str1);
        }
        OLED_ShowNum(1, 1, K210.leftnumber1, 3);
        OLED_ShowNum(2, 1, K210.leftnumber2, 3);
    }
}

#endif

#if TEST == NRF24L01_T

int main()
{
    uint8_t j = 0;
    uint8_t k = 0;
    uint8_t *str = NULL;
    POWER_turnAll(off);
    OLED_turn(on);
    UART3_turn(on);
    Delay_ms(100);
    OLED_Init();
    OLED_ShowString(3, 1, "123");
    TIMBASE_TIM4_Init();
    K210_Init(USART3, 115200);
    // MPU6050_init(I2C1);
    // I2C_init(I2C1, MPU6050_SLAVE_ADDRESS_0);
    while (1)
    {
        k++;
        OLED_ShowNum(1, 1, j, 3);
        OLED_ShowNum(1, 5, k, 3);
        while(!K210_sendByte(0xAA));
        Delay_ms(10);
        if (K210_receiveString(&str))
        {
            j++;
            OLED_ShowHexNum(4, 1, str[0], 2);
            OLED_ShowHexNum(4, 4, str[1], 2);
            OLED_ShowHexNum(4, 7, str[2], 2);
            OLED_ShowHexNum(4, 10, str[3], 2);
        }
        // MPU6050_BUF[0] = 10;
        // I2C_pointer(I2C1)->BUF[0] = 10;
        // I2C_readReg(I2C1, INT_PIN_CFG, 1);
        // MPU6050_waitBusy();
        // I2C_waitBusy(I2C1);
        // OLED_ShowSignedNum(2, 1, MPU6050.yaw - MPU6050.GYROZ_0, 5);
        // OLED_ShowNum(2, 1, I2C_pointer(I2C1)->BUF[0], 3);
    }
}

#endif

#if TEST == NORMAL

int main(void)
{
    uint8_t i = 0;
    int16_t gyz = 0;
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    OLED_Init();
    OPENMV_init(USART2, 115200);
    MPU6050_init(I2C2, 50000, I2C_DutyCycle_2); // the initiation of mpu6050 must precede timebase unit
    TIMBASE_TIM4_Init();
    Encoder_TIM1_Init();
    Encoder_TIM2_Init();
    Serial_Init();
    OLED_ShowString(1, 1, "Hello Kenny");
    PWM_Init();

    Read_Flash(0x0801FC00, 1);
    if (middle == 8)
    {
        Read_Flash(0x08011000, 1);
        task_state = middle;
    }

    while (1)
    {
        Medicine();
        OLED_ShowNum(2, 1, Yao_Flag, 2);
        if (numberfrom_k210 == 0) // controlled by k210
        {
            USART_Cmd(USART1, ENABLE);
            numberfrom_k210 = read_k210(); // k210 recognizes number
            USART_Cmd(USART1, DISABLE);
        }
        if ((Yao_Flag == 1 && numberfrom_k210 != 0 && old_Yao_Flag == 2) | (Yao_Flag == 0 && numberfrom_k210 != 0 && old_Yao_Flag == 1) | (Yao_Flag == 1 && numberfrom_k210 != 0 && old_Yao_Flag == 1))
        {
            old_Yao_Flag = 1;
            switch (task_state)
            {
            case 0:
                break; // 空闲状态
            case 1:
                task1();
                break; // 任务1：
            case 2:
                task2();
                break; // 任务2：
                //						case 3:	 task3(); 		 break;   	//任务3：
            default:
                break;
            }
        }
    }
}

#endif

#if TEST == TIM

int main()
{
    uint8_t gyrp = 0;
    uint8_t i = 0;
    uint32_t second = 1;
    GPIOC_myTest_init();
    OPENMV_init(USART1, 115200);
    // MPU6050_init(I2C1, 50000, I2C_DutyCycle_2, GPIO_Speed_50MHz);
    // while (!OPENMV_sendString("123"))
    // ;
    while (1)
    {
        if (TIM2_mills() == second * 1000)
        {
            second++;
            i++;
            while (!OPENMV_sendByte(i))
                ;
            while (!OPENMV_sendByte(i))
                ;
            while (!OPENMV_sendByte(i))
                ;
            while (!OPENMV_sendByte(i))
                ;
            while (!OPENMV_sendByte(i))
                ;
            while (!OPENMV_sendByte(i))
                ;
            while (!OPENMV_sendByte(i))
                ;
            while (!OPENMV_sendByte(i))
                ;
            while (!OPENMV_sendByte(i))
                ;
            while (!OPENMV_sendByte(i))
                ;
        }
    }
}

#endif

#if TEST == MPU6050_T

int main()
{
    uint8_t i = 0;
    GPIOC_myTest_init();
    OLED_Init();
    MPU6050_init(I2C2, 90000, I2C_DutyCycle_2);

    MPU6050_waitBusy();
    MPU6050_exist = false;

    Delay_ms(200);
    TIMBASE_TIM4_Init();
    while (1)
    {
        i++;
        // MPU6050_readReg(MPU6050_DATA, 14);
        // MPU6050_getBUFData();
        OLED_ShowSignedNum(4, 1, MPU6050.gyro_z, 3);
        OLED_ShowSignedNum(3, 1, MPU6050.GYROZ_0, 3);
        OLED_ShowSignedNum(2, 1, angle, 3);
        OLED_ShowSignedNum(2, 6, MPU6050.x, 3);
        OLED_ShowSignedNum(3, 6, MPU6050.y, 3);
        OLED_ShowSignedNum(4, 6, MPU6050.z, 3);
        // MPU6050_readByte(WHO_AM_I);
        // gyz = MPU6050_BUF;
        OLED_ShowNum(1, 1, i, 3);
        // OLED_ShowSignedNum(2, 1, (int16_t)angle, 5);
        // OLED_ShowNum(3, 1, mills, 10);
        // OLED_ShowNum(4, 1, gyz, 3);
        // OLED_ShowNum(4, 7, busytime, 5);
    }
}

#endif

#if TEST == L298N

int main()
{
    uint8_t i;
    GPIOC_myTest_init();
    L298N_init();
    L298N_setDuty(0x40);
    L298N_setIN(IN1 | IN3, ENABLE);
    OLED_init(GPIO_Pin_8, GPIO_Pin_9);
    OLED_clear();
    while (1)
    {
        i++;
        OLED_showbinary(0, i);
    }
}

#endif

#if TEST == OPENMV_T

int main()
{
    uint8_t i = 0;
    uint8_t j;
    bool car_turnleft = false;
    GPIOC_myTest_init();
    // NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    OPENMV_init(USART2, 115200);
    MPU6050_init(I2C2, 90000, I2C_DutyCycle_2);
    MPU6050_exist = false;
    MPU6050_waitBusy();
    OLED_Init();
    TIMBASE_TIM4_Init();
    Encoder_TIM1_Init();
    Encoder_TIM2_Init();
    Serial_Init();
    OLED_ShowString(1, 1, "Hello Kenny");
    PWM_Init();

    PWM_L = PWM_R = 75;
    car_callibrate = true;

    while (1)
    {
        if (car_callibrate)
        {
            MPU6050_exist = false;
            openmv_exist = true;
            callibrate_setup();
            while (car_callibrate)
            {
                i++;
                OLED_ShowSignedNum(1, 1, OPENMV.xa, 3);
                OLED_ShowNum(1, 6, OPENMV.turnflag, 1);
                OLED_ShowSignedNum(2, 1, OPENMV.xb, 3);
                OLED_ShowSignedNum(3, 1, MPU6050.yaw, 3);
                OLED_ShowNum(4, 1, mills, 3);
                OLED_ShowNum(4, 5, OPENMV_bufAvailable, 3);
                callibrate_loop();
                if (OPENMV.turnflag == 1)
                {
                    car_go(60);
                    car_callibrate = false;
                    car_turnleft = true;
                    Delay_ms(250);
                }
            }
        }
        if (car_turnleft)
        {
            openmv_exist = false;
            MPU6050_exist = true;
            left_ninetydegrees();
            MPU6050_exist = false;
            MPU6050_waitBusy();
            car_callibrate = true;
        }
    }
}

#endif

#if TEST == POWER

int main()
{
    uint8_t j = 0;
    // UART2_turn(on);
    POWER_turnAll(on);
    OLED_turn(on);
    OLED_Init();
    while (1)
    {
        j++;
        OLED_ShowBinNum(1, 1, j, 8);
        Delay_ms(100);
        if (j == 200)
        {
            POWER_turnAll(off);
            OLED_turn(on);
        }
    }
}

#endif
